import math
import os.path

import numpy as np
import pandas as pd

# 计算每两个点之间的相对极坐标
def latlon_to_polar_relative(lat1, lon1, lat2, lon2):
    R = 6378137.0  # 地球半径（单位：米）

    # 转换为弧度制
    phi1 = math.radians(lat1)
    phi2 = math.radians(lat2)
    delta_phi = math.radians(lat2 - lat1)
    delta_lambda = math.radians(lon2 - lon1)

    # Haversine公式计算距离
    a = math.sin(delta_phi / 2) ** 2 + math.cos(phi1) * math.cos(phi2) * math.sin(delta_lambda / 2) ** 2
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
    distance = R * c

    # 计算角度（方位角）
    y = math.sin(delta_lambda) * math.cos(phi2)
    x = math.cos(phi1) * math.sin(phi2) - math.sin(phi1) * math.cos(phi2) * math.cos(delta_lambda)
    angle = math.atan2(x, y)

    #return distance, math.degrees(angle)
    return distance, angle / np.pi


# 假设你的GPS数据在一个CSV文件中，包含纬度和经度

def save_relative_polar(head):
    #head = './dataset/No_81_Denggao_Road-2024-10-03_05-19-03'
    loca_path = os.path.join(head, 'Location.csv')
    acce_path = os.path.join(head, 'Accelerometer.csv')
    location_data = pd.read_csv(loca_path)
    #location_data = pd.read_csv('./Location.csv')
    time, latitudes, longitudes = location_data['time'], location_data['latitude'], location_data['longitude']

    distances = []
    angles = []

    # 从第二个点开始计算相对于前一个点的位移和角度
    index = int(len(latitudes) * 0.01) + 1
    time = time[index:len(time)-index]
    for i in range(index, len(latitudes)-index):
    #for i in range(index, index+5):
        lat1, lon1 = latitudes[i - 1], longitudes[i - 1]
        lat2, lon2 = latitudes[i], longitudes[i]

        distance, angle = latlon_to_polar_relative(lat1, lon1, lat2, lon2)
        distances.append(distance)
        angles.append(angle)
    #print(len(distances))
    # 保存结果到DataFrame中
    polar_data = pd.DataFrame({
        'time':time,
        'distance': distances,
        'angle': angles
    })

    # 查看转换后的极坐标数据
    print(polar_data.head())
    # 保存转换后的极坐标数据
    polar_data.to_csv(os.path.join(head,"relative_polar_coordinates.csv"), index=False)


dir = '../dataset'
data = os.listdir(dir)
for d in data:
    head = os.path.join(dir, d)
    save_relative_polar(head)


